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Abstract 



We present a novel spectral learning algorithm for simultaneous localization and 
mapping (SLAM) from range data with known correspondences. This algorithm 
is an instance of a general spectral system identification framework, from which it 
inherits several desirable properties, including statistical consistency and no local 
optima. Compared with popular batch optimization or multiple-hypothesis track- 
ing (MHT) methods for range-only SLAM, our spectral approach offers guaran- 
teed low computational requirements and good tracking performance. Compared 
with popular extended Kalman filter (EKF) or extended information filter (EIF) 
approaches, and many MHT ones, our approach does not need to linearize a tran- 
sition or measurement model; such linearizations can cause severe errors in EKFs 
and EIFs, and to a lesser extent MHT, particularly for the highly non-Gaussian 
posteriors encountered in range-only SLAM. We provide a theoretical analysis of 
our method, including finite-sample error bounds. Finally, we demonstrate on a 
real-world robotic SLAM problem that our algorithm is not only theoretically jus- 
tified, but works well in practice: in a comparison of multiple methods, the lowest 
errors come from a combination of our algorithm with batch optimization, but our 
method alone produces nearly as good a result at far lower computational cost. 

1 Introduction 

In range-only SLAM, we are given a sequence of range measurements from a robot to fixed land- 
marks, and possibly a matching sequence of odometry measurements. We then attempt to simulta- 
neously estimate the robot's trajectory and the locations of the landmarks. Popular approaches to 
range-only SLAM include EKFs and EIFs (Kantor & Singh, 2002; Kurth et al., 2003; Djugash & 
Singh, 2008; Djugash, 2010; Thrun et al., 2005), multiple-hypothesis trackers (including particle 
filters and multiple EKFs/EIFs) (Djugash et al., 2005; Thrun et al., 2005), and batch optimization of 
a likelihood function (Kehagias et al., 2006). 

In all the above approaches, the most popular representation for a hypothesis is a list of landmark 
locations (m n , x ,m n . y ) and a list of robot poses (xt,yt,6t). Unfortunately, both the motion and 
measurement models are highly nonlinear in this representation, leading to computational problems: 
inaccurate linearizations in EKF/EIF/MHT and local optima in batch optimization approaches (see 
Section|2]for details). Much work has attempted to remedy this problem, e.g., by changing the hy- 
pothesis representation (Djugash, 2010) or by keeping multiple hypotheses (Djugash et al., 2005; 
Djugash, 2010; Thrun et al., 2005). While considerable progress has been made, none of these 
methods are ideal; common difficulties include the need for an extensive initialization phase, inabil- 
ity to recover from poor initialization, lack of performance guarantees, or excessive computational 
requirements. 

We take a very different approach: we formulate range-only SLAM as a matrix factorization prob- 
lem, where features of observations are linearly related to a 4- or 7-dimensional state space. This 
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approach has several desirable properties. First, we need weaker assumptions about the measure- 
ment model and motion model than previous approaches to SLAM. Second, our state space yields 
a linear measurement model, so we hope to lose less information during tracking to approximation 
errors and local optima. Third, our formulation leads to a simple spectral learning algorithm, based 
on a fast and robust singular value decomposition (SVD) — in fact, our algorithm is an instance of 
a general spectral system identification framework, from which it inherits desirable guarantees in- 
cluding statistical consistency and no local optima. Fourth, we don't need to worry as much as 
previous methods about errors such as a consistent bias in odometry, or a receiver mounted at a 
different height from the transmitters: in general, we can learn to correct such errors automatically 
by expanding the dimensionality of our state space. 

As we will discuss in Section |2j our approach to SLAM has much in common with spectral algo- 
rithms for subspace identification (Van Overschee & De Moor, 1996; Boots et al., 2010); unlike 
these methods, our focus on SLAM makes it easy to interpret our state space. Our approach is also 
related to factorization-based structure from motion (Tomasi & Kanade, 1992; Triggs, 1996; Kanade 
& Morris, 1998), as well as to recent dimensionality -reduction-based methods for localization and 
mapping (Shang et al., 2003; Biggs et al., 2005; Ferris et al., 2007; Yairi, 2007). 

We begin in Section [2] by reviewing background related to our approach. In Section [5] we present 
the basic spectral learning algorithm for range-only SLAM, and discuss how it relates to state space 
discovery for a dynamical system. We conclude in Section|4]by comparing spectral SLAM to other 
popular methods for range-only SLAM on real world range data collected from an autonomous 
lawnmower with time-of-flight ranging radios. 



2 Background 

There are four main pieces of relevant background: first, the well-known solutions to range-only 
SLAM using variations of the extended Kalman filter and batch optimization; second, recently- 
discovered spectral approaches to identifying parameters of nonlinear dynamical systems; third, 
matrix factorization for finding structure from motion in video; and fourth, dimensionality-reduction 
methods for localization and mapping. Below, we will discuss the connections among these areas, 
and show how they can be unified within a spectral learning framework. 



2.1 Likelihood-based Range-only SLAM 

The standard probabilistic model for range-only localization (Kantor & Singh, 2002; Kurth et al., 
2003) represents robot state by a vector s t — [xt,yt, #t] T ; the robot's (nonlinear) motion and obser- 
vation models are 



St+l 



x t + v t cos(6> t ) 
y t + v t sin(6> t ) 
Ot + Ut 



H d tl n = \J (rn n . x - x t ) 2 + {m n , y - TJt) 2 + Vt (1) 



Here v t is the distance traveled, ui t is the orientation change, d t n is the estimate of the range from 
the nth landmark location (m n>x , rn ntV ) to the current location of the robot (xt,yt), and et and T] t 
are noise. (Throughout this paper we assume known correspondences, since range sensing systems 
such as radio beacons typically associate unique identifiers with each reading.) 

To handle SLAM rather than just localization, we can extend the state to include landmark positions: 

st = [%t, Vt,6t, mi lX ,mi lV , m N . x ,m N , y ] T (2) 

where N is the number of landmarks. The motion and measurement models remain the same. 
Given this model, we can use any standard optimization algorithm (such as Gauss-Newton) to fit the 
unknown robot and landmark parameters by maximum likelihood. Or, we can track these parameters 
online using EKFs, EIFs, or MHT methods like particle filters. 

EKFs and EIFs are a popular solution for localization and mapping problems: for each new odome- 
try input at = [vt,0Jt] T and each new measurement d t , we propagate the estimate of the robot state 
and error covariance by linearizing the non-linear motion and measurement models. Unfortunately, 
though, range-only SLAM is notoriously difficult for EKFs/EIFs: since range-only sensors are not 
informative enough to completely localize a robot or a landmark from a small number of readings, 
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Figure 1 : A general principle for state space discovery. We can think of state as a statistic of history 
that is minimally sufficient to predict future observations. If the bottleneck is a rank constraint, then 
we get a spectral method. 



nonlinearities are much worse in range-only SLAM than they are in other applications such as range- 
and-bearing SLAM. In particular, if we don't have a sharp prior distribution for landmark positions, 
then after a few steps, the exact posterior becomes highly non-Gaussian and multimodal; so, any 
Gaussian approximation to the posterior is necessarily inaccurate. Furthermore, an EKF will gen- 
erally not even produce the best possible Gaussian approximation: a good linearization would tell 
us a lot about the modes of the posterior, which would be equivalent to solving the original SLAM 
problem. So, practical applications of the EKF to range-only SLAM attempt to delay linearization 
until enough information is available, e.g., via an extended initialization phase for each landmark. 
Such delays simply push the problem of finding a good hypothesis onto the initialization algorithm. 

Djugash et al. proposed a polar parameterization to more accurately represent the annular and mul- 
timodal distributions typically encountered in range-only SLAM. The resulting approach is called 
the ROP-EKF, and is shown to outperform the ordinary (Cartesian) EKF in several real-world prob- 
lems, especially in combination with multiple-hypothesis tracking (Djugash & Singh, 2008; Dju- 
gash, 2010). However, the multi-hypothesis ROP-EKF can be much more expensive than an EKF, 
and is still a heuristic approximation to the true posterior. 

Instead of the posterior covariance of the state (as used by the EKF), the extended information fil- 
ter (EIF) maintains an estimate of the inverse covariance. The two representations are statistically 
equivalent (and therefore have the same failure modes). But, the inverse covariance is often approx- 
imately sparse, leading to much more efficient approximate computation (Thmn et al., 2005). 



2.2 Spectral State Space Discovery and System Identification 

System identification algorithms attempt to learn dynamical system parameters such as a state space, 
a dynamics model (motion model), and an observation model (measurement model) directly from 
samples of observations and actions. In the last few years, spectral system identification algorithms 
have become popular; these algorithms learn a state space via a spectral decomposition of a care- 
fully designed matrix of observable features, then find transition and observation models by linear 
regressions involving the learned states. Originally, subspace identification algorithms were almost 
exclusively used for linear system identification (Van Overschee & De Moor, 1996), but recently, 
similar spectral algorithms have been used to learn models of partially observable nonlinear dynam- 
ical systems such as HMMs (Hsu et al., 2009; Siddiqi et al., 2010) and PSRs (Rosencrantz et al., 
2004; Boots et al., 2010; Boots & Gordon, 2010; Boots et al., 201 1). All of these spectral algorithms 
share a strategy for state space discovery: they learn a state space via a spectral decomposition of 
a matrix of observations (Figure [TJ, resulting in a linear observation function, and then they learn a 
model of the dynamics in the learned low-dimensional state space. This is a powerful and appealing 
approach: the resulting algorithms are statistically consistent, and they are easy to implement with 
efficient linear algebra operations. In contrast, batch optimization of likelihood (e.g., via the popu- 
lar expectation maximization (EM) algorithm) is only known to be consistent if we find the global 
optimum of the likelihood function — typically an impractical requirement. 
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As we will see in Section [3] we can view the range-only SLAM problem as an instance of spectral 
state space discovery. And, the Appendix (Sec. |6.3| l discusses how to identify transition and mea- 
surement models given the learned states. The same properties that make spectral methods appealing 
for system identification carry over to our spectral SLAM algorithm: computational efficiency, sta- 
tistical consistency, and finite-sample error bounds. 



2.3 Orthographic Structure From Motion 

In some ways the orthographic structure from motion (SfM) problem in vision (Tomasi & Kanade, 
1992) is very similar to the SLAM problem: the goal is to recover scene geometry and camera 
rotations from a sequence of images (compare with landmark geometry and robot poses from a 
sequence of range observations). And in fact, one popular solution for SfM is very similar to the 
state space discovery step in spectral state space identification. The key idea in spectral SfM is that 
is that an image sequence can be represented as a 2F x P measurement matrix W, containing the 
horizontal and vertical coordinates of P points tracked through F frames. If the images are the 
result of an orthographic camera projection, then it is possible to show that rank(Vl / ) = 3. As a 
consequence, the measurement matrix can be factored into the product of two matrices U and V, 
where U contains the 3d positions of the features and V contains the camera axis rotations (Tomasi 
& Kanade, 1992). With respect to system identification, it is possible to interpret the matrix U as 
an observation model and V as an estimate of the system state. Inspired by SfM, we reformulate 
range-only SLAM problem in a similar way in Section[3j and then similarly solve the problem with a 
spectral learning algorithm. Also similar to SfM, we examine the identifiability of our factorization, 
and give a metric upgrade procedure which extracts additional geometric information beyond what 
the factorization gives us. 



2.4 Dimensionality-reduction-based Methods for Mapping 

Dimensionality reduction methods have recently provided an alternative to more traditional 
likelihood-based methods for mapping. In particular, the problem of finding a good map can be 
viewed as finding a (possibly nonlinear) embedding of sensor data via methods like multidimen- 
sional scaling (MDS) and manifold learning. 

For example, MDS has been used to determine a Euclidean map of sensor locations where there is 
no distinction between landmark positions and robot positions (Shang et al., 2003): instead all-to- 
all range measurements are assumed for a set of landmarks. If some pairwise measurements are not 
available, these measurements can be approximated by some interpolation method, e.g. the geodesic 
distance between the landmarks (Tenenbaum et al., 2000; Shang et al., 2003). 

Our problem differs from this previous work: in contrast to MDS, we have no landmark-to-landmark 
measurements and only inaccurate robot-to-robot measurements (from odometry, which may not be 
present, and which often has significant errors when integrated over more than a short distance). Ad- 
ditionally, our smaller set of measurements introduces additional challenges not present in classical 
MDS: linear methods can recover the positions only up to a linear transformation. This ambiguity 
forces changes compared to the MDS algorithm: while MDS factors the all-to-all matrix of squared 
ranges, in Sec. |3.1| we factor only a block of this matrix, then use either a metric upgrade step or a 
few global position measurements to resolve the ambiguity. 

A popular alternative to linear dimensionality reduction techniques like classical MDS is manifold 
learning: nonlinearly mapping sensor inputs to a feature space that "unfolds" the manifold on which 
the data lies and then applying dimensionality reduction. Such nonlinear dimensionality reduction 
has been used to learn maps of wi-fi networks and landmark locations when sensory data is thought 
to be nonlinearly related to the underlying Eucidean space in which the landmarks lie (Biggs et al., 
2005; Ferris et al., 2007; Yairi, 2007). Unlike theses approaches, we show that linear dimensionality 
reduction is sufficient to solve the range-only SLAM problem. (In particular, (Yairi, 2007) suggests 
solving range-only mapping using nonlinear dimensionality reduction. We not only show that this is 
unnecessary, but additionally show that linear dimensionality reduction is sufficient for localization 
as well.) This greatly simplifies the learning algorithm and allows us to provide strong statistical 
guarantees for the mapping portion of SLAM (Sec. |3.3| >. 
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3 State Space Discovery and Spectral SLAM 



We start with SLAM from range data without odometry. For now, we assume no noise, no missing 
data, and batch processing. We will generalize below: Sec. |3.2| discusses how to recover robot 
orientation, Sec.|3.3|discusses noise, and Sec. 3.4 discusses missing data and online SLAM. In the 



Appendix (Section 6.3 i we discuss learning motion and measurement models. 



3.1 Range-only SLAM as Matrix Factorization 

Consider the matrix Y € M ArxT of squared ranges, with N > 4 landmarks and T > 4 time steps: 



Y = 



1 



d 2 d 2 

"11 "12 



4i 



d 2 

NX 



d 2 

"22 



d 2 
a N2 



d 2 

d 2 T 



d 2 



(3) 



where d n _ t is the measured distance from the robot to landmark n at time step t. 

The most basic version of our spectral SLAM method relies on the insight that Y factors according 
to robot position (xt,yt) and landmark position {m n ,xi m nt y). To see why, note 



( m l,x + m l,y) ~ 2m n,x ■ X t 



2m. 



Vt 



Vt 



(4) 



If we write C n = [(m^ + m^ y )/2, m ra>K) m„ >3/ , 1] T and X t = [1, -x u -y t , {xf + y\ )/2] T , it is 



easy to see that d„ t 
of landmarks, 



2ClX t . So, Y factors as Y = CX, where C G 



i>Nx4 



C 



Kx+% 2 ,j)/ 2 m i,^ 

(m| +m| )/2 7712^ 



mi,v 

m 2,j/ 



( m W,^ + m N,y)/ 2 m N,x m N . y 



contains the positions 



(5) 



and X G M 4xT contains the positions of the robot over time 



X = 



1 

-Xi 

-yi 

(*? + V?)/2 



1 

-VT 

Vy\)l2 



(6) 



If we can recover C and X, we can read off the solution to the SLAM problem. The fact that Y's 
rank is at most 4 suggests that we might be able to use a rank-revealing factorization of Y, such as the 
singular value decomposition, to find C and X. Unfortunately, such a factorization only determines 
C and X up to a linear transform: given an invertible matrix S, we can write Y = CX = CS~ 1 SX. 
Therefore, factorization can only hope to recover U = C5 -1 and V = SX. 

To upgrade the factors U and V to a full metric map, we have two options. If global position 
estimates are available for at least four landmarks, we can learn the transform S via linear regression, 
and so recover the original C and X. This method works as long as we know at least four landmark 
positions. Figure|2]\ shows a simulated example. 

On the other hand, if no global positions are known, the best we can hope to do is recover landmark 
and robot positions up to an orthogonal transform (translation, rotation, and reflection). It turns out 
that Eqs. |5}|6]) provide enough additional geometric constraints to do so: in the Appendix (Sec. |6.1[ ) 
we show that, if we have at least 9 time steps and at least 9 landmarks, and if each of these point 
sets is non-singular in an appropriate sense, then we can compute the metric upgrade in closed form. 
The idea is to fit a quadratic surface to the rows of U, then change coordinates so that the surface 
becomes the function in Q. (By contrast, the usual metric upgrade for orthographic structure from 
motion (Tomasi & Kanade, 1992), which uses the constraint that camera projection matrices are 
orthogonal, requires a nonlinear optimization.) 
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Figure 2: Spectral SLAM on simulated data. See Section 4.1 for details. A.) Randomly gener- 
ated landmarks (6 of them) and robot path through the environment (500 timesteps). A SVD of the 
squared distance matrix recovers a linear transform of the landmark and robot positions. Given the 
coordinates of 4 landmarks, we can recover the landmark and robot positions in their original coordi- 
nates; or, since 500 > 9, we can recover positions up to an orthogonal transform with no additional 
information. Despite noisy observations, the robot recovers the true path and landmark positions 
with very high accuracy. B.) The convergence of the observation model C^q for the remaining two 
landmarks: mean Frobenius-norm error vs. number of range readings received, averaged over 1000 
randomly generated pairs of robot paths and environments. Error bars indicate 95% confidence 
intervals. 



3.2 SLAM with Headings 

In addition to location, we often want the robot's global heading 9. We could get headings by post- 
processing our learned positions, but in practice we can reduce variance by learning positions and 
headings simultaneously. We do so by adding more features to our measurement matrix: differences 
between successive pairs of squared distances, scaled by velocity (which we can estimate from 
odometry). Since we need pairs of time steps, we now have Y £ M 2NxT ~ 1 : 



Y 



d 2 

a Nl 



d\ 2 



d 2 

N2 



d 2 

U 1T-1 



d 2 

a NT- 



l ir "it-i 



NT "iVT-1 



(7) 



As before, we can factor Y into a robot state matrix and a landmark matrix. The key new observation 
is that we can write the new features in terms of cos(#) and sin(6*): 



t+i 



Vt) 



x 



t+i 



xf + yt +1 - yf 



2v t 



Vi 



cos(6> t ) - m ni y sm{d t ) + 



x i+i ~ x t + Vt+i ~ Vt 



2v t 



(8) 



From Eq. [4] and Eq. [8] it is easy to see that Y has rank at most 7 (exactly 7 if the robot path and 
landmark positions are not singular): we have Y = CX, where C £ E JVx7 contains functions of 
landmark positions and X £ R 7xT contains functions of robot state, 
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Algorithm 1 Spectral SLAM 



In: i.i.d. pairs of observations {o tl a t }f =1 ; optional: measurement model for > 4 landmarks C\a 
Out: measurement model (map) C, robot locations X (the tth column is location at time t) 

1: Collect observations and odometry into a matrix Y (Eq.|?} 

2: Find the the top 7 singular values and vectors: (U, A, V T ) <- SVD(F, 7) 

The transformed measurement matrix is CS -1 — U and robot states are SX = AV T . 

3: Find S via linear regression (from U to C\ a) or metric upgrade (see Appendix) 
and return C = US and X = S^AV 1 " 



C 



(mf x +mf y )/2 mi,! m\, y 10 
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(10) 



As with the basic SLAM algorithm in Section [XT we can factor Y using SVD, this time keeping 7 
singular values. To make the state space interpretable, we can then look at the top part of the learned 
transform of C: as long as we have at least four landmarks in non-singular position, this block will 
have exactly a three-dimensional nullspace (due to the three columns of zeros in the top part of 
C). After eliminating this nullspace, we can proceed as before to learn S and make the state space 
interpretable: either use the coordinates of at least 4 landmarks as regression targets, or perform a 
metric upgrade. (See the Appendix, Sec. 6.1 for details). Once we have positions, we can recover 
headings as angles between successive positions. 



3.3 A Spectral SLAM Algorithm 



The matrix factorizations of Sees. |3. l| and 3.2 suggest a straightforward SLAM algorithm, Alg. [T] 
build an empirical estimate Y of Y by sampling observations as the robot traverses its environment, 
then apply a rank-7 thin SVD, discarding the remaining singular values to suppress noise. 



(U,A,V T ) <- SVD(f,7) 



(11) 



Following Section 



3.2 



the left singular vectors U are an estimate of our transformed measurement 
matrix CS^ 1 , and the weighted right singular vectors AV T are an estimate of our transformed robot 
state SX. We can then learn S via regression or metric upgrade. 

Statistical Consistency and Sample Complexity Let M e R NxN be the true observation co- 
variance for a randomly sampled robot position, and let M = ^YY T be the empirical covariance 
estimated from T observations. Then the true and estimated measurement models are the top singu- 
lar vectors of M and M. Assuming that the noise in M is zero-mean, as we include more data in 
our averages, we will show below that the law of large numbers guarantees that M converges to the 
true covariance M. So, our learning algorithm is consistent for estimating the range of M, i.e., the 
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landmark locations. (The estimated robot positions will typically not converge, since we typically 
have a bounded effective number of observations relevant to each robot position. But, as we see 
each landmark again and again, the robot position errors will average out, and we will recover the 
true map.) 

In more detail, we can give finite-sample bounds on the error in recovering the true factors. For 
simplicity of presentation we assume that noise is i.i.d., although our algorithm will work for any 
zero-mean noise process with a finite mixing time. (The error bounds will of course become weaker 
in proportion to mixing time, since we gain less new information per observation.) The argument 



(see the Appendix, Sec. 6.2 for details) has two pieces: standard concentration bounds show that 
each element of our estimated covariance approaches its population value; then the continuity of the 
SVD shows that the learned subspace also approaches its true value. The final bound is: 



sm#|| 2 < (12) 



where W is the vector of canonical angles between the learned subspace and the true one, c is a 
constant depending on our error distribution, and 7 is the true smallest nonzero eigenvalue of the 
covariance. In particular, this bound means that the sample complexity is 0(( 2 ) to achieve error £. 



3.4 Extensions: Missing Data, Online SLAM, and System ID 

Missing data So far we have assumed that we receive range readings to all landmarks at each time 
step. In practice this assumption is rarely satisfied: we may receive range readings asynchronously, 
some range readings may be missing entirely, and it is often the case that odometry data is sampled 
faster than range readings. Here we outline two methods for overcoming this practical difficulty. 

First, if a relatively small number of observations are missing, we can use standard approaches for 
factorization with missing data. For example, probabilistic PCA (Tipping & Bishop, 1999) estimates 
the missing entries via an EM algorithm, and matrix completion (Candes & Plan, 2009) uses a trace- 
norm penalty to recover a low-rank factorization with high probability. However, for range-only 
data, often the fraction of missing data is high and the missing values are structural rather than 
random. 

The second approach is interpolation: we divide the data into overlapping subsets and then use 
local odometry information to interpolate the range data within each subset. To interpolate the data, 
we estimate a robot path by dead reckoning. For each point in the dead reckoning path we build 
the feature representation [1, —x, —y, (x 2 + y 2 )/2] T . We then learn a linear model that predicts 
a squared range reading from these features (for the data points where range is available), as in 
Eq. 4] Next we predict the squared range along the entire path. Finally we build the matrix Y by 
averaging the locally interpolated range readings. This interpolation approach works much better in 
practice than the fully probabilistic approaches mentioned above, and was used in our experiments 
in Section|4] 



Online Spectral SLAM The algorithms developed in this section so far have had an important 
drawback: unlike many SLAM algorithms, they are batch methods not online ones. The extension 
to online SLAM is straightforward: instead of first estimating Y and then performing a SVD, we 
sequentially estimate our factors (U, A, V T ) via online SVD (Brand, 2006; Boots et al., 201 1). 



Robot Filtering and System Identification So far, our algorithms have not directly used (or 
needed) a robot motion model in the learned state space. However, an explicit motion model is re- 
quired if we want to predict future sensor readings or plan a course of action. We have two choices: 
we can derive a motion model from our learned transformation S between latent states and physical 
locations, or we can learn a motion model directly from data using spectral system identification. 
More details about both of these approaches can be found in the Appendix, Sec. |6.3| 
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Figure 3: The autonomous lawn mower and spectral SLAM. A.) The robotic lawn mower platform. 
B.) In the first experiment, the robot traveled 1.9km receiving 3,529 range measurements. This path 
minimizes the effect of heading error by balancing the number of left turns with an equal number 
of right turns in the robot's odometry (a commonly used path pattern in lawn mowing applications). 
The light blue path indicates the robot's true path in the environment, light purple indicates dead- 
reckoning path, and dark blue indicates the spectral SLAM localization result. C.) In the second 
experiment, the robot traveled 1.3km receiving 1,816 range measurements. This path highlights the 
effect of heading error on dead reckoning performance by turning in the same direction repeatedly. 
Again, spectral SLAM is able to accurately recover the robot's path. 



4 Experimental Results 



We perform several SLAM and robot navigation experiments to illustrate and test the ideas pro- 
posed in this paper. First we show how our methods work in theory with synthetic experiments 
where complete observations are received at each point in time and i.i.d. noise is sampled from a 
multivariate Gaussian distribution. Next we demonstrate our algorithm on data collected from a 
real-world robotic system with substantial amounts of missing data. Experiments were performed 
in Matlab, on a 2.66 GHz Intel Core i7 computer with 8 GB of RAM. In contrast to batch nonlinear 
optimization approaches to SLAM, the spectral learning methods described in this paper are very 
fast, usually taking less than a second to run. 



4.1 Synthetic Experiments 



Our simulator randomly places 6 landmarks in a 2-D environment. A simulated robot then randomly 
moves through the environment for 500 time steps and receives a range reading to each one of the 
landmarks at each time step. The range readings are perturbed by noise sampled from a Gaussian 
distribution with variance equal to 1% of the range. Given this data, we apply the algorithm from 



Section 3.3 to solve the SLAM problem. We use the coordinates of 4 landmarks to learn the linear 
transform S and recover the true state space, as shown in Figure|2j\. The results indicate that we can 
accurately recover both the landmark locations and the robot path. 

We also investigated the empirical convergence rate of our observation model (and therefore the 
map) as the number of range readings increased. To do so, we generated 1000 different random 
pairs of environments and robot paths. For each pair, we repeatedly performed our spectral SLAM 
algorithm on increasingly large numbers of range readings and looked at the difference between our 
estimated measurement model (the robot's map) and the true measurement model, excluding the 
landmarks that we used for reconstruction: 1 1 C 5:6 — C^\\j=. The results are shown in Figure [2J3, 
and show that our estimates steadily converge to the true model, corroborating our theoretical results 
(in Section[3~3"1and the Appendix). 
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4.2 Robotic Experiments 



We used two freely available range-only SLAM data sets collected from an autonomous lawn mow- 
ing robot (Djugash, 2010), shown in Fig.[3j\[^]These "Plaza" datasets were collected via radio nodes 
from Multispectral Solutions that use time-of-flight of ultra-wide-band signals to provide inter-node 
ranging measurements. (Additional details on the experimental setup can be found in (Djugash, 
2010).) This system produces a time-stamped range estimate between the mobile robot and station- 
ary nodes (landmarks) in the environment. The landmark radio nodes are placed atop traffic cones 
approximately 138cm above the ground throughout the environment, and one node was placed on 
top of the center of the robot's coordinate frame (also 138cm above the ground). The robot odometry 
(dead reckoning) comes from an onboard fiberoptic gyro and wheel encoders. The two environmen- 
tal setups, including the locations of the landmarks, the dead reckoning paths, and the ground truth 
paths, are shown in Figure[3]3-C. The ground truth paths have 2cm accuracy according to (Djugash, 
2010). 

The two Plaza datasets that we used to evaluate our algorithm have very different characteristics. 
In "Plaza 1," the robot travelled 1.9km, occupied 9,658 distinct poses, and received 3,529 range 
measurements. The path taken is a typical lawn mowing pattern that balances left turns with an 
equal number of right turns; this type of pattern minimizes the effect of heading error. In "Plaza 
2," the robot travelled 1.3km, occupied 4,091 poses, and received 1,816 range measurements. The 
path taken is a loop which amplifies the effect of heading error. The two data sets were both very 
sparse, with approximately 1 1 time steps (and up to 500 steps) between range readings for the worst 



landmark. We first interpolated the missing range readings with the method of Section 3.4 Then we 
applied the rank-7 spectral SLAM algorithm of Section [3~3| the results are depicted in Figure[3j}-C. 
Qualitatively, we see that the robot's localization path conforms to the true path. 

In addition to the qualitative results, we quantitatively compared spectral SLAM to a number of dif- 
ferent competing range-only SLAM algorithms. The localization root mean squared error (RMSE) 
in meters for each algorithm is shown in Figure |4] The baseline is dead reckoning (using only 
the robot's odometry information). Next are several standard online range-only SLAM algorithms, 
summarized in (Djugash, 2010). These algorithms included the Cartesian EKF, FastSLAM (Monte- 
merlo et al., 2002) with 5,000 particles, and the ROP-EKF (Djugash & Singh, 2008). These previous 
results only reported the RMSE for the last 10% of the path, which is typically the best 10% of the 
path (since it gives the most time to recover from initialization problems). The full path localization 
error can be considerably worse, particularly for the initial portion of the path — see Fig. 5 (right) 
of (Djugash & Singh, 2008). 

We also compared to batch nonlinear optimization, via Gauss-Newton as implemented in Matlab's 
fminunc (see (Kehagias et al., 2006) for details). This approach to solving the range-only SLAM 
problem can be very data efficient, but is subject to local optima and is very computationally inten- 
sive. We followed the suggestions of (Kehagias et al., 2006) and initialized with the dead-reckoning 
estimate of the robot's path. The algorithm took roughly 2.5 hours to converge on Plaza 1, and 
45 minutes to converge on Plaza 2. Under most evaluation metrics, the nonlinear batch algorithm 
handily beats the EKF-based alternatives. 

Finally, we ran our spectral SLAM algorithm on the same data sets. In contrast to Gauss-Newton, 
spectral SLAM is statistically consistent, and much faster: the bulk of the computation is the fixed- 
rank SVD, so the time complexity of the algorithm is 0((2N) 2 T) where N is the number of land- 
marks and T is the number of time steps. Empirically, spectral SLAM produced results that were 
comparable to batch optimization in 3-4 orders of magnitude less time (see Figure^. 

Spectral SLAM can also be used as an initialization procedure for nonlinear batch optimization. This 
strategy combines the best of both algorithms by allowing the locally optimal nonlinear optimization 
procedure to start from a theoretically guaranteed good starting point. Therefore, the local optimum 
found by nonlinear batch optimization should be no worse than the spectral SLAM solution and 
likely much better than the batch optimization seeded by dead-reckoning. Empirically, we found 
this to be the case (Figure [4]). If time and computational resources are scarce, then we believe that 
spectral SLAM is clearly the best approach; if computation is not an issue, the best results will almost 



http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/index.html 
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Method 


Plaza 1 


Plaza 2 


Dead Reckoning (full path) 


15.92m 


27.28m 


Cartesian EKF (last, best 10%) 


0.94m 


0.92m 


FastSLAM (last, best 10%) 


0.73m 


1.14m 


ROP EKF (last, best 10%) 


0.65m 


0.87m 


Batch Opt. (worst 10%) 


1.04m 


0.45m 


Batch Opt. (last 10%) 


1.01m 


0.45m 


Batch Opt. (best 10%) 


0.56m 


0.20m 


Batch Opt. (full path) 


0.79m 


0.33m 


Spectral SLAM (worst 10%) 


1.01m 


0.51m 


Spectral SLAM (last 10%) 


0.98m 


0.51m 


Spectral SLAM (best 10%) 


0.59m 


0.22m 


Spectral SLAM (full path) 


0.79m 


0.35m 


Spectral + Batch Optimization (worst 10%) 


0.89m 


0.40m 


Spectral + Batch Optimization (last 10%) 


0.81m 


0.32m 


Spectral + Batch Optimization (best 10%) 


0.54m 


0.18m 


Spectral + Batch Optimization (full path) 


0.69m 


0.30m 



Runtime (seconds) 




"■ 7 3 I I 0.51 

Plaza 1 Plaza 2 

Figure 4: Comparison of Range-Only SLAM Algorithms. The table shows Localization RMSE. 
Spectral SLAM has localization accuracy comparable to batch optimization on its own. The best 
results (boldface entries) are obtained by initializing nonlinear batch optimization with the spectral 
SLAM solution. The graph compares runtime of Gauss-Newton batch optimization with spectral 
SLAM. Empirically, spectral SLAM is 3-4 orders of magnitude faster than batch optimization on 
the autonomous lawnmower datasets. 



certainly be found by refining the spectral SLAM solution using a nonlinear batch optimization 
procedure. 



5 Conclusion 



We proposed a novel solution for the range-only SLAM problem that differs substantially from 
previous approaches. The essence of this new approach is to formulate SLAM as a factorization 
problem, which allows us to derive a local-minimum free spectral learning method that is closely 
related to SfM and spectral approaches to system identification. We provide theoretical guarantees 
for our algorithm, discuss how to derive an online algorithm, and show how to generalize to a full 
robot system identification algorithm. Finally, we demonstrate that our spectral approach to SLAM 
beats other state-of-the-art SLAM approaches on real-world range-only SLAM problems. 
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6 Appendix 



6.1 Metric Upgrade for Learned Map 

In the main body of the paper, we assumed that global position estimates of at least four landmarks 
were known. When these landmarks are known, we can recover all of the estimated landmark 
positions and robot locations. 

In many cases, however, no global positions are known; the best we can hope to do is recover 
landmark and robot positions up to an orthogonal transform (translation, rotation, and reflection). 
It turns out that Eqs. ([5j|6]l provide enough geometric constraints to perform this metric upgrade, 
as long as we have at least 9 landmarks and at least 9 time steps, and as long as C and X are 
nonsingular in the following sense: define the matrix Ci, with the same number of rows as C but 10 
columns, whose zth row has elements CijC^k for 1 < j < k < 4 (in any fixed order). Note that the 
rank of C2 can be at most 9: from Eq. |5j we know that cf 2 + c f 3 ~ 2c^4 = 0, and each of the three 
terms in this function is a multiple of a column of Ci- We will say that C is nonsingular if C2 has 
rank exactly 9, i.e., is rank deficient by exactly 1 dimension. The conditions for X are analogous, 
swapping rows for columns j^j 

To derive the metric upgrade, suppose that we start from an N x 4 matrix U of learned landmark 



coordinates and an 4 x N matrix V of learned robot coordinates from the algorithm of Sec. 3.1 
And, suppose that we have at least 9 nonsingular landmarks and robot positions. We would like to 
transform the learned coordinates into two new matrices C and X such that 

ci « 1 (13) 



1 9 1 9 

c 4 « + 2 C ' (14) 



x A 1 (15) 



1 



where c is a row of C and x is a column of X. 

At a high level, we first fit a quadratic surface to the rows of U, then transform this surface so 
that it satisfies Eq. [T3]jl4| and scale the surface so that it satisfies Eq. [15] Our surface will then 



automatically also satisfy Eq. 16 since X must be metrically correct if C is. 



In more detail, we first (step i) linearly transform each row of U into approximately the form 
(1, 1,7*1,2, Ti.3): we use linear regression to find a coefficient vector a £ ]R 4 such that Ua « 1, 
then set R = UQ where Q 6 M 4x3 is an orthonormal basis for the nullspace of a T . After this step, 
our factorization is (UT X ) (Tf 1 V), where T x = (a Q). 

Next (step ii) we fit an implicit quadratic surface to the rows of R by finding 10 coefficients bjk (for 
< j < k < 3) such that 

b n r li + &i2?"i,m,2 + h3n,in :3 + 6 2 2^ j2 + 623^,2^3 + ^33^3 

To do so, we form a matrix S that has the same number of rows as U but 10 columns. The elements 
of row i of S are ,Tj & for < j < k < 3 (in any fixed order). Here, for convenience, we define 
r i,o = 1 for a U *■ Then we find a vector b G R w that is approximately in the nullspace of S T by 
taking a singular value decomposition of S and selecting the right singular vector corresponding to 
the smallest singular value. Using this vector, we can define our quadratic as w hr T Hr+£ T r+boo, 
where r is a row of R, and the Hessian matrix H and linear part t are given by: 




H = I ^21 5&22 &23 



bi2 b 13 

23 

' J 32 9^33 







) - 









2 For intuition, a set of landmarks or robot positions that all lie on the same quadratic surface (line, circle, 
parabola, etc.) will be singular. Some higher-order constraints will also lead to singularity; e.g., a set of points 
will be singular if they all satisfy |(xf + yf)xi + Hi = 0, since each of the two terms in this function is a 
column of Ci- 
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Over the next few steps we will transform the coordinates in R to bring our quadratic into the form 
of Eq.[l4j that is, one coordinate will be a quadratic function of the other two, there will be no linear 
or constant terms, and the quadratic part will be spherical with coefficient |. 

We start (step iii) by transforming coordinates so that our quadratic has no cross-terms, i.e., so 
that its Hessian matrix is diagonal. Using a 3 x 3 singular value decomposition, we can factor 
H = MH'M T so that M is orthonormal and W is diagonal. If we set B! = RM and I' = Ml, and 
write r' for a row of R', we can equivalently write our quadratic as = \ (r') T H'r' + (^') T r' + 6oo> 
which has a diagonal Hessian as desired. After this step, our factorization is (t/Tir 2 )(r 2 " 1 Tf V), 
where 

T _ ( 1 

12 - \ M 

Our next step (step iv) is to turn our implicit quadratic surface into an explicit quadratic function. 
For this purpose we pick one of the coordinates of R' and write it as a function of the other two. In 
order to do so, we must have zero as the corresponding diagonal element of the Hessian H' — else we 
cannot guarantee that we can solve for a unique value of the chosen coordinate. So, we will take the 
index j such that H'^ is minimal, and set H'^ = 0. Suppose that we pick the last coordinate, j = 3. 
(We can always reorder columns to make this true; SVD software will typically do so automatically.) 
Then our quadratic becomes 



1 



H' 22 {r' 2 f+^ 



v 3' 3 



+ h 



00 



H' 22 {r' 2 f 



tW 2 



Now (step v) we can shift and rescale our coordinates one more time to get our quadratic in the 
desired form: translate so that the linear and constant coefficients are 0, and rescale so that the 
quadratic coefficients are \. For the translation, we define new coordinates r" = r' + c for c £ K 3 , 
so that our quadratic becomes 



C3 



1 



#ii K - ci) 2 + \H' 22 {4 - c 2 f + tM d) + £' 2 (r> 2 ' c 2 ) + h 



on 



By expanding and matching coefficients, we know c must satisfy 



#22 



C 2 



(coefficient of r") 
(coefficient of r' 2 ' ) 



= c 3 - 



21' ' 



#22 

2& ' 



Cl 



|c 2 - 6 00 /4 



(constant) 



The first two equations are linear in c\ and c 2 (and don't contain C3). So, we can solve directly for c\ 
and c 2 ; then we can plug their values into the last equation to find C3. For the scaling, the coefficient 

■ " ■ 2|f, and that of r 2 is now — ^fr 



of r'l is now 



So, we can just scale these two coordinates 



separately to bring their coefficients to \. 

After this step, our factorization is U'V, where U' = UTiT 2 T 3 and V = T^T^T^V, and 



la 



( 1 










Cl 










C2 





4_ 





V C3 








1 J 



The left factor U' will now satisfy Eq. 13 14 We still have one last useful degree of freedom: if we 
set C = U'T A , where 



1 














/' 














/' 














/< 2 
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for any /i G M, then C will still satisfy Eq. 13 14 So (step vi), we will pick /i to satisfy Eq. 15 in 
particular, we set fi = ^/mean(V^' .), so that when we set X = T 4 ~ 1 V', the last row of X will have 
mean 1. 



If we have 7 learned coordinates in U as in Sec. |3.2| we need to find a subspace of 4 coordinates 
in order to perform metric upgrade. To do so, we take advantage of the special form of the correct 
answer, given in Eq. [9} in the upper block of C in Eq.|9] three coordinates are identically zero. Since 
U is a linear transformation of C, there will be three linear functions of the top block of U that are 
identically zero (or approximately zero in the presence of noise). As long as the landmark positions 
are nonsingular, we can use SVD on the top block of U to find and remove these linear functions 
(by setting the smallest three singular values to zero), then proceed as above with the four remaining 
coordinates. 



6.2 Sample Complexity for the Measurement Model (Robot Map) 



Here we provide the details on how our estimation error scales with the number T of training 
examples — that is, the scaling of the difference between the estimated measurement model U, which 
contains the location of the landmarks, and its population counterpart. 

Our bound has two parts. First we use a standard concentration bound (the Azuma-Hoeffding in- 
equality) to show that each element of our estimated covariance M = YY T approaches its popula- 
tion value. We start by rewriting the empirical covariance matrix as a vector summed over multiple 
samples: 



vec 



(m)4£t : 



where T = (Y Q Y) T is the matrix of column-wise Kronecker products of the observations Y. We 
assume that each element of T minus its expectation ET^ is bounded by a constant c; we can derive 
c from bounds on anticipated errors in distance measurements and odometry measurements. 

ITi.t-ET,! <c, V M 

Then the Azuma-Hoeffding inequality bounds the probability that the empirical sum differs too 
much from its population value: for any a > and any i, 



T 

E 



(T ilt -ET0 



> a 



< 2e- a2 ' 2Tc2 



If we pick a = \/2Tc 2 log(T), then we can rewrite the probability in terms of T: 

T 

- £(T i)t -ET<) 



> c 



21og(T) 



T 



which means that the probability decreases as O(^) and the threshold decreases as 0(^=). 
We can then use a union bound over all (2A^) 2 covariance elements (since Y € M. 2NxT ): 



t=i 



EX, 



> c 



21og(T) 
T 



< 8N 2 /T 



That is, with high probability, the entire empirical covariance matrix M will be close (in max-norm) 
to its expectation. 

Next we use the continuity of the SVD to show that the learned subspace approaches its true value. 
Let M = M + E, where E is the perturbation (so the largest element of E is bounded). Let U be 
the output of SVD, and let U be the population value (the top singular vectors of the true M). Let $ 
be the matrix of canonical angles between range (J7) and range(f/). Since we know the exact rank 
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of the true M (either 4 or 7), the last (4th or 7th) singular value of M will be positive; call it 7 > 0. 
So, by Theorem 4.4 of Stewart and Sun (Stewart & Sun, 1990), 



sin* 2 < 



7 



This result uses a 2-norm bound on E, but the bound we showed above is in terms of the largest 
element of E. But, the 2-norm can be bounded in terms of the largest element: 

||£|| 2 < Nmsx.\E, 

ij 

Finally, the result is that we can bound the canonical angle: 



Nc 



21og(T) 



II sin*|| 2 < 

7 

In other words, the canonical angle shrinks at a rate of 0(— U), with probability at least 1 — 



6.3 The Robot as a Nonlinear Dynamical System 



Once we have learned an interpretable state space via the algorithm of Section 3.3 we can simply 



write down the nominal robot dynamics in this space. The accuracy of the resulting model will 
depend on how well our sensors and actuators follow the nominal dynamics, as well as how well we 
have learned the transformation S to the interpretable version of the state space. 

In more detail, we model the robot as a controlled nonlinear dynamical system. The evolution is 
governed by the following state space equations, which generalize Q: 

f(s u a t ) + e t (17) 

(18) 



St+l 



o t = h(s t ) + v t 

Here St G M. k denotes the hidden state, a t € R l denotes the control signal, ot € M m denotes the 
observation, e t & K fe denotes the state noise, and v t G M. m denotes the observation noise. For our 
range-only system, following the decomposition of Section[3] we have: 



Si 



1 

-x t 

-Vt 

(s? + «?)/2 
-cos(0 t ) 

- sin(0 t ) 

2 2 1 2 2 

2v t 



Ot = 



4J2 



2v t 



2v t 



, at 



v t 

cos(w t ) 
sin(cJt) 



(19) 



Here v t and ui t are the translation and rotation calculated from the robot's odometry. A nice property 
of this model is that expected observations are a linear function of state: 

h(s t ) = Cs t (20) 

The dynamics, however, are nonlinear, see Eq. |2T| which can easily be derived from the basic 
kinematic motion model for a wheeled robot (Thrun et al., 2005). 



f(st,a t ) 



2 1 2 



1 

x t - v t cos(0 t ) 
■y t - v t sin(0 t ) 

_ h vtyt sm(9 t ) H 2 

cos(0 t ) cos(w t ) + s'm(0 t ) sin(wt) 



- v t x t cos(9 t 



^ 2 cos 2 (e t )+ti t 2 sin 2 (e f ) 



— sin(0 t ) cos(ujt) + cos(9 t ) sm(cj t ) 
[x t cos(9 t ) cos(wt) — x t sin(0 t ) sin(a; t ) + v t cos 2 (# t ) cos(w t ) 
y t sm(9 t ) cos(w 4 ) - y t sin(w t ) cos(6» t ) + v t sm 2 (6 t ) cos(w t ) - 
2v t cos(0 t ) sm(6 t ) sin(w t )] 



(21) 
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6.3.1 Robot System Identification 



To apply the model of Section 6.3 it is essential that we maintain states in the physical coordinate 
frame, and not just the linearly transformed coordinate frame — i.e., C and not U — CS^ 1 . So, to 
use this model, we must first learn S either by regression or by metric upgrade. 

However, it is possible instead to use system identification to learn to filter directly in the raw state 
space U. We conjecture that it may be more robust to do so, since we will not be sensitive to errors 
in the metric upgrade process (errors in learning S), and since we can learn to compensate for some 
deviations from the nominal model of Section EOl 

To derive our system identification algorithm, we can explicitly rewrite f(s t ,a t ) as a nonlinear 
feature-expansion map followed by a linear projection. Our algorithm will then just be to use linear 
regression to learn the linear part of /. 



First, let's look at the dynamics for the special case of S = I. Each additive term in Eq. 21 is the 
product of at most two terms in s t and at most two terms in at- Therefore, we define 4>(st, a t ) := 
St (8 St ® a t <g> at, where a t — [1, a*] T and <g) is the Kronecker product. (Many of the dimensions 
of 4>(st,at) are duplicates; for efficiency we would delete these duplicates, but for simplicity of 



notation we keep them.) Each additive term in Eq. 21 is a multiple of an element of <p(st,at), so we 
can write the dynamics as: 

s t +i = N<f)(s u a t ) + e t (22) 



where N is a linear function that picks out the correct entries to form Eq. 21 



Now, given an invertible matrix S, we can rewrite f(st, at ) as an equivalent function in the trans- 
formed state space: 

Ss t+ i = f(Ss t , a t ) + Se t (23) 

To do so, we use the identity (Ax) ® (By) = (A<8) B)(x ® y). Repeated application yields 

4>(Ss t , a t ) = Ss t ® Ss t ®a t ®a t 

= (S ® S ® I <E) I)(s t ® s t ® a t ® a t ) 

= S0(s u a t ) (24) 

where S = S®S®I<g>I. Note that S is invertible (since rank(A <g> B) = rank(A) rank(S)); so, 
we can write 

f(Ss u a t ) = SNS- 1 §4>(h, at) = Sf(s tl a t ) (25) 

Using this representation, we can learn the linear part of /, SNS -1 , directly from our state esti- 
mates: we just do a linear regression from <fi(Sst, at) to Sst+i- 

For convenience, we summarize the entire learning algorithm (state space discovery followed by 
system identification) as Algorithm[2] 

6.3.2 Filtering with the Extended Kalman Filter 

Whether we leam the dynamics through system identification or simply write them down in the 



interpretable version of our state space, we will end up with a transition model of the form ( 22 1 



and an observation model of the form (20i. Given these models, it is easy to write down an EKF 
which tracks the robot state. The measurement update is just a standard Kalman filter update (see, 
e.g., (Thrun et al., 2005)), since the observation model is linear. For the motion update, we need a 
Taylor approximation of the expected state at time t + 1 around the current MAP state s t , given the 
current action at : 

s t+ i - s t « N[4>(s t ,at) + f s \. t (st- h)] (26) 

~£\ g = (s® I + I® s) ®at ® at (27) 

We simply plug this Taylor approximation into the standard Kalman filter motion update 
(e.g., (Thrun et al., 2005)). 
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Algorithm 2 Robot System Identification 

In: T i.i.d. pairs of observations {ot, at}f = i, measurement model for 4 landmarks C14 (by e.g. 
GPS) 

Out: measurement model C, motion model N, robot states A (the ith column is state St) 



Collect observations and odometry into a matrix Y (Eq. [7} 

Find the the top 7 singular values and vectors: (U, A, V T ) 4- SVD(Y, 7) 

Find the transformed measurement matrix CS^ 1 = U and robot states SX = AV T 

Compute a matrix $ with columns $ t = <f>(Sst, at). 

Compute dynamics: SNS^ 1 = S'Jf2:r(^i:T-i) t 

Compute the partial 5 -1 : = C^Ci^S'" 1 ) where CS"" 1 comes from st ep 3. S 1 A gives 



3.2 • 



us the x, y coordinates of the states. These can be used to find X (see Section . 
1: Given A, we can compute the full S as S — (5A)A^ 

8: Finally, from steps 3,5, and 7, we find the interpretable measurement model (CS~ 1 )S and 
motion model N = S^iSNS'^S. 
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